#include "daq.h"
#include "comPort.h"
#include "util/util.h"
#include "global.h"
#include <stdio.h>
#include <vector>

using namespace std;
using namespace DAQ_namespace;


void ringBuffer::flush()
{
    FTError( FT_Purge(handle, FT_PURGE_RX | FT_PURGE_TX) );
    this->tail = this->bufferBegin;
    this->head = this->bufferBegin;
    this->bytesInBuffer = 0;
}

/* ---<>-----------------------------------------------------------------------------------
	ringBuffer() constructor
-------------------------------------------------------------------------------------------- */
ringBuffer::ringBuffer()
{
    this->bufferBegin = &this->data[0];
    this->bufferEnd = &this->data[bufferSize-1];
    this->bytesInBuffer = 0;
    this->dataOverFlow = false;
    this->head = &this->data[0];
    this->tail = &this->data[0];
    this->vTail = &this->data[0];
}


/* ---<>-----------------------------------------------------------------------------------
		bufferRead
Ring buffer read function data is read from the buffer buffer tail pointer is incremented
bytesInBuffer is decremented
-------------------------------------------------------------------------------------------- */
uint8_t ringBuffer::bufferRead()
{
    if (tail == bufferEnd){
        uint8_t temp = *tail;
        tail = bufferBegin;
        bytesInBuffer--;
        return temp;
    }
    else
    {
        bytesInBuffer--;
        return *tail++;
    }
}

uint8_t ringBuffer::vBufferRead()
{
    if (vTail == bufferEnd){
        uint8_t temp = *vTail;
        vTail = bufferBegin;
        bytesInBuffer--;
        return temp;
    }
    else
    {
        bytesInBuffer--;
        return *vTail++;
    }
}

/* ---<>-----------------------------------------------------------------------------------
		bufferWrite
Ring buffer write function data is placed in the buffer buffer head 
pointer is incremented bytesInBuffer is incremented
-------------------------------------------------------------------------------------------- */
void ringBuffer::bufferWrite(char c)
{
    if(head == bufferEnd)
	{
        *head = c;
        head = bufferBegin;
        if(bytesInBuffer >= bufferSize) dataOverFlow = true;
        bytesInBuffer++;
    }
    else
    {
        if (bytesInBuffer >= bufferSize) dataOverFlow = true;
        bytesInBuffer++;
        *head++ = c;
    }
}

status_t ringBuffer::receiveFunc()
{
    DWORD       rxBytes = 0,txBytes = 0;
    DWORD       eventWord;
    FT_STATUS   status;
    uint8_t     tempBuffer[4096];
    DWORD       bytesRead;


    //printf("RX entered\r\n");

        FTError(FT_GetStatus(handle,&rxBytes,&txBytes,&eventWord));
        if(rxBytes > 0)
        {
    //        printf("number of bytes to read = %d\r\n",rxBytes);
            FT_SetTimeouts(handle, 2, 2);
            status = FT_Read(handle,tempBuffer,rxBytes,&bytesRead);
            while(rxBytes < bytesRead);
            if(status == FT_OK)
            {
                uint16_t i;
                for(i=0;i<rxBytes;i++)
                {
                    //send data read to the ring buffer
                    bufRx.bufferWrite(tempBuffer[i]); //need to make sure we pass this right
    //                printf("0x%X\r\n",tempBuffer[i]);
                }
                if(bufRx.dataOverFlow) printf("dataOverFlow");
                return OPP_SUCCESSFUL;
            }
            else
            {
                printf("read failed");
                return OPP_FAILED;
            }

        }
        else
        {
            return OPP_FAILED;
        }
    //printf("receive finished\r\n");
}

DAQALLOC::DAQALLOC()
{
//Analog In

    this->analogIn[0].settingPending    = false;
    this->analogIn[0].enable            = false;
    this->analogIn[0].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[0].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[0].resolution        = ADC_14_BIT_gc;
    this->analogIn[0].sign              = true;

    this->analogIn[1].settingPending    = false;
    this->analogIn[1].enable            = false;
    this->analogIn[1].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[1].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[1].resolution        = ADC_14_BIT_gc;
    this->analogIn[1].sign              = true;

    this->analogIn[2].settingPending    = false;
    this->analogIn[2].enable            = false;
    this->analogIn[2].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[2].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[2].resolution        = ADC_14_BIT_gc;
    this->analogIn[2].sign              = true;

    this->analogIn[3].settingPending    = false;
    this->analogIn[3].enable            = false;
    this->analogIn[3].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[3].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[3].resolution        = ADC_14_BIT_gc;
    this->analogIn[3].sign              = true;

//Analog Out

    this->analogOut[0].enable           = false;
    this->analogOut[0].settingPending   = false;

//Digital

    for(int i = 0;i<13;i++)
    {
        this->digital.inSettingPending[i]    = false;
        this->digital.outSettingPending[i]   = false;
    }
    this->digital.inEnable              = false;
    this->digital.outEnable             = false;

    for(int i=0;i<13;i++)
    {
        this->digital.dioAlloc[i] = FEATURE_FREE;
        this->digital.src[i] = DIO_FREE;
    }
//Encoder

    this->encoder[0].settingPending     = false;
    this->encoder[0].absolute           = false;
    this->encoder[0].enable             = false;
    this->encoder[0].index              = false;
    this->encoder[0].PLOC               = PLOC_NULL;
    this->encoder[0].resolution         = 0;

    this->encoder[1].settingPending     = false;
    this->encoder[1].absolute           = false;
    this->encoder[1].enable             = false;
    this->encoder[1].index              = false;
    this->encoder[1].PLOC               = PLOC_NULL;
    this->encoder[1].resolution         = 0;

//Motor
    this->motor[0].settingPending       = false;
    this->motor[0].enable               = false;
    this->motor[1].settingPending       = false;
    this->motor[1].enable               = false;
    this->motor[2].settingPending       = false;
    this->motor[2].enable               = false;
    this->motor[3].settingPending       = false;
    this->motor[3].enable               = false;

//Servo
    this->servo[0].settingPending       = false;
    this->servo[0].enable               = false;
    this->servo[1].settingPending       = false;
    this->servo[1].enable               = false;
    this->servo[2].settingPending       = false;
    this->servo[2].enable               = false;
    this->servo[3].settingPending       = false;
    this->servo[3].enable               = false;

//General

    this->general.startPending          = false;
    this->general.stopPending           = false;
    this->general.actuatorSuperPending  = false;
    this->general.outputSuperPending    = false;

//Timer

    this->timer.enable                  = false;
    this->timer.loopTime                = 0;
    this->timer.settingPending          = false;

//State machine
    this->state                         = DAQ_BOOT_STATE;
}

status_t DAQALLOC::sendAdcInit(uint8_t adcMsk, GAIN_t adcGain)
{
    adcInit_t packet;

    packet.Header         = packetHeader;
    packet.msgHeader      = adcInit_mh;
    packet.PARAM1.ACHM    = adcMsk;
    packet.PARAM1.GAIN    = adcGain;

    //set pending flags for every affected channel
    if(adcMsk & ADC_CH0_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[0].settingPending = true;
        this->analogIn[0].gain = adcGain;
    }
    if(adcMsk & ADC_CH1_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[1].settingPending = true;
        this->analogIn[1].gain = adcGain;
    }
    if(adcMsk & ADC_CH2_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[2].settingPending = true;
        this->analogIn[2].gain = adcGain;
    }
    if(adcMsk & ADC_CH3_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[3].settingPending = true;
        this->analogIn[3].gain = adcGain;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(adcInit_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(adcInit_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDacEnable(uint8_t DACM)
{

    dacEnable_t packet;

    packet.Header           = packetHeader;
    packet.msgHeader        = dacEnable_mh;
    packet.PARAM1.DCHM      = DACM;

    if(DACM == DAC_CH0_bp)
    {
        //see if a setting is pending
        if(this->analogOut[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogOut[0].settingPending = true;
    }
    if(DACM == DAC_CH1_bp)
    {
        //see if a setting is pending
        if(this->analogOut[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogOut[1].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(dacEnable_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(dacEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDigitalInEnable(uint16_t data)
{
    WORD_t val;

    digitalInEnable_t packet;

    packet.Header     = packetHeader;
    packet.msgHeader  = digitalInEnable_mh;

    //check for a pending setting
    for(int i = 0; i<13;i++)
    {
        if(data & 1<<i){
            if(this->digital.inSettingPending[i])
            {
                return OPP_FAILED;
            }
        }
    }


    for(int i = 0;i<13;i++)
    {
        if(data & 1<<i)
        {
            this->digital.inSettingPending[i] = true;
        }
    }


    val.word = data;

    packet.dataH = val.bytes.byteH;
    packet.dataL = val.bytes.byteL;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(digitalInEnable_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(digitalInEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDigitalOutEnable(uint16_t data)
{

    WORD_t val;

    digitalOutEnable_t packet;

    packet.Header     = packetHeader;
    packet.msgHeader  = digitalOutEnable_mh;

    //check for a pending setting
    for(int i = 0; i<13;i++)
    {
        if(data & 1<<i){
            if(this->digital.outSettingPending[i])
            {
                return OPP_FAILED;
            }
        }
    }

    for(int i = 0;i<13;i++)
    {
        if(data & 1<<i)
        {
            this->digital.outSettingPending[i] = true;
        }
    }

    val.word = data;

    packet.dataH = val.bytes.byteH;
    packet.dataL = val.bytes.byteL;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(digitalOutEnable_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(digitalOutEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendEncoderInit(PLOC_ENUM PLOC, bool index, uint8_t num, bool ABS, DIGFILT_ENUM EDFL, uint16_t resolution)
{
    encInit_t packet;

    WORD_t res;

    res.word = resolution;

    packet.Header             = packetHeader;
    packet.msgHeader          = encInit_mh;

    packet.PARAM1.ENCN        = num;
    packet.PARAM1.INDX        = index;
    packet.PARAM1.PLOC        = PLOC;

    packet.PARAM2.ABS         = ABS;
    packet.PARAM2.EDFL        = EDFL;
    packet.PARAM2.ERESH       = res.bytes.byteH & 0x0F;
    packet.PARAM3.ERESL       = res.bytes.byteL;

    if(num == 0)
    {
        if(this->encoder[0].settingPending)
        {
            return OPP_FAILED;
        }

        this->encoder[0].settingPending = true;
        this->encoder[0].absolute = ABS;
        this->encoder[0].index = index;
        this->encoder[0].PLOC = PLOC;
        this->encoder[0].resolution = resolution;
    }
    if(num == 1)
    {
        if(this->encoder[1].settingPending)
        {
            return OPP_FAILED;
        }

        this->encoder[1].settingPending = true;
        this->encoder[1].absolute = ABS;
        this->encoder[1].index = index;
        this->encoder[1].PLOC = PLOC;
        this->encoder[1].resolution = resolution;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(encInit_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(encInit_mh));

    return OPP_SUCCESSFUL;

}

status_t DAQALLOC::sendMotorEnable(uint8_t msk)
{
    motorEnable_t packet;

    packet.Header                 = packetHeader;
    packet.msgHeader              = motorEnable_mh;

    packet.PARAM1.motorEn_msk     = msk & 0xF;

    if(msk & 0x01)
    {
        if(this->motor[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[0].settingPending = true;
    }
    if(msk & 0x02)
    {
        if(this->motor[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[1].settingPending = true;
    }
    if(msk & 0x04)
    {
        if(this->motor[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[2].settingPending = true;
    }
    if(msk & 0x08)
    {
        if(this->motor[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[3].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(motorEnable_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(motorEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendServoEnable(uint8_t msk)
{
    servoEnable_t packet;

    packet.Header                 = packetHeader;
    packet.msgHeader              = servoEnable_mh;

    packet.PARAM1.servoEn_msk     = msk & 0xF;

    if(msk & 0x01)
    {
        if(this->servo[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[0].settingPending = true;
    }
    if(msk & 0x02)
    {
        if(this->servo[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[1].settingPending = true;
    }
    if(msk & 0x04)
    {
        if(this->servo[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[2].settingPending = true;
    }
    if(msk & 0x08)
    {
        if(this->servo[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[3].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(servoEnable_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(servoEnable_mh));
    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendSetTimerFreq(uint16_t freq)
{
    tmrSetFreq_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = tmrSetFreq_mh;

    packet.frequency    = freq;

    if(this->timer.settingPending)
    {
        return OPP_FAILED;
    }
    this->timer.settingPending = true;
    this->timer.loopTime = freq;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(tmrSetFreq_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(tmrSetFreq_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendStartProgram()
{
    programStart_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = startProgram_mh;

    if(this->general.startPending)
    {
        return OPP_FAILED;
    }
    this->general.startPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(startProgram_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(startProgram_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendStopProgram()
{
    programStop_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = stopProgram_mh;

    if(this->general.stopPending)
    {
        //return OPP_FAILED;
    }

    this->general.stopPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(stopProgram_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(stopProgram_mh));

    return OPP_SUCCESSFUL;
}

//Always pass full 4 value deep arrays, un-allocated actuator values will be disregarded
status_t DAQALLOC::sendActuatorSuperPacket(uint16_t servoPosition[], uint16_t motorEffort[])
{
    uint8_t packet[this->getActuatorSuperPacketLength()];

    WORD_t val;

    packet[0] = packetHeader;
    packet[1] = actuatorSuperPacket_mh;

    uint8_t pos = 2,inc;

    if(this->general.actuatorSuperPending)
    {
        return OPP_FAILED;
    }

    //SERVOS
    for(inc=0;inc<4;inc++)
    {
        if(this->servo[inc].enable)
        {
            val.word = servoPosition[inc];
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }
    }

    //MOTORS
    for(inc=0;inc<4;inc++)
    {
        if(this->motor[inc].enable)
        {
            val.word = motorEffort[inc];
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }
    }

    crcAppend(&packet[0],getActuatorSuperPacketLength());
    comPortPut(&packet[0],getActuatorSuperPacketLength());

    return OPP_SUCCESSFUL;

}

status_t DAQALLOC::sendOutputSuperPacket(uint16_t analogOut[], uint16_t dioSet, uint16_t dioClr)
{
    uint8_t packet[this->getOutputSuperPacketLength()];

    WORD_t val;

    packet[0] = packetHeader;
    packet[1] = outputSuperPacket_mh;

    uint8_t pos = 2,inc;

    if(this->general.outputSuperPending)
    {
        return OPP_FAILED;
    }

    //Analog
    for(inc=0;inc<2;inc++)
    {
        if(this->analogOut[inc].enable)
        {
            val.word = analogOut[inc];
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }
    }

    //Digital Set
        if(this->digital.outEnable)
        {
            val.word = dioSet;
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;

            val.word = dioClr;
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }

    crcAppend(&packet[0],getOutputSuperPacketLength());
    comPortPut(&packet[0],getOutputSuperPacketLength());

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::inputEnabled()
{
    for(int i=0; i<13;i++)
    {
        if(this->digital.dioAlloc[i] == FEATURE_UTILIZED && this->digital.src[i] == DIGITAL_INPUT)
        {
            return OPP_SUCCESSFUL;
        }
    }
    return OPP_FAILED;
}
status_t DAQALLOC::outputEnabled()
{
    for(int i=0; i<13;i++)
    {
        if(this->digital.dioAlloc[i] == FEATURE_UTILIZED && this->digital.src[i] == DIO_OUTPUT)
        {
            return OPP_SUCCESSFUL;
        }
    }
    return OPP_FAILED;
}
status_t DAQALLOC::sendReset()
{
    resetDaq_t packet;

    packet.Header = packetHeader;
    packet.msgHeader = resetDaq_mh;
    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(resetDaq_mh));
    comPortPut((uint8_t *)&packet.Header,packetSizeLookup(resetDaq_mh));
    return OPP_SUCCESSFUL;
}
